from functools import reduce
import math
from TestShipBase import TSimShipBase, Pool
from TestShipBase import calcTrueHeading, calcDistanceHaversine, calcTargetPosition


pool = Pool(maxlen = 10)
'''
Pool 数据缓冲池:
    缓冲池的大小设置后固定，例如为10，即len(pool) = 10，表示记录最近10个时刻的系统状态数据
    每个时刻的系统状态数据包括其中每条船的状态,这里表示为 [{$ship1$}, {$ship2$}, {$ship3$}, ...]
    那么这个缓冲池在运行时内部的结构如下所示：

        pool = [
            {'time': time1, mmsi1: {$ship1$}, mmsi2: {$ship2$}, ...},
            {'time': time2, mmsi1: {$ship1$}, mmsi2: {$ship2$}, ...},
            ...
            {'time': timen, mmsi1: {$ship1$}, mmsi2: {$ship2$}, ...},
        ]

    其中 {$ship1$} 示例:
    {
        "Time": 120, 
        "type": 15,
        "mmsi": "2130001",
        "length": 22.0, 
        "width": 7.0, 
        "sName": "Ferry0.48704",
        "lon": 118.60009834252573, 
        "lat": 31.939050966970306, 
        "head": 241.63576226204026, 
        "rot": 0.0, 
        "speed": 3.876, 
        "next": 33, 
        "nextLon": 118.62957717009711, 
        "nextLat": 31.947045209517565
    }
'''

class TSimShipFerry(TSimShipBase):
    def __init__(self, Data):
        super().__init__(Data = Data)
        self.initData(Data)

    def initData(self, Data = {}):
        # 下面添加此类 "额外的" 参数
        #jfzhang增加的参数
        self.RUDDER_MAX = 35       #舵角的最大值，取35°，不需要保存到数据库
        self.K         = 0.0579    #旋回性指数，每条船舶是固定的，不需要保存到数据库
        self.T         = 69.9784   #追随性指数，每条船舶是固定的，不需要保存到数据库
        self.delta     = Data.get('delta', 0.0) #船舶在当前时刻的舵角，正值为右转，负值为左转
        self.rot_old  = 0.0       #船舶上一时刻的角速度，单位是°/s，如何更新需要确定
        self.rot      = Data.get('rot', 0.0)  #船舶当前时刻的角速度，单位是°/s
        self.sName = Data.get('sName', 'shipName')
        #记录船舶与其他船舶之间碰撞风险最大的那条船数据
        self.risk_ship = None      #碰撞风险最大船舶的MMSI
        self.collision_risk = None #碰撞风险值
        self.DCPA = None           #DCPA值
        self.TCPA = None           #TCPA值

    # 重写saveData 以定制返回
    def saveData(self):
        return {
                "Time": self.getTime(), 
                "type": self.type,
                "mmsi": self.id,
                "length": self.length, 
                "width": self.width, 
                "sName": self.sName,
                "lon": self.lon, 
                "lat": self.lat, 
                "head": self.head, 
                "rot": self.rot, 
                "speed": self.speed, 
                "next": self.next, 
        }

    # 刘炯炯负责开发，放在智能船子类里面
    def AntiCollision(self):
        # data = pool[-1]
        # for ship in data:
        #     mmsi = ship['mmsi']
        #     mmsi = ship.get('mmsi', '21000')
        #     pass
        # TODO
        # 避碰决策，调整设定新的head和rot等参数
        # 直接修改 head, speed, rot等
        pass

    def NavigateModel(self):
        '''
        # 此方法可能不需要修改
        # 驾驶决策：根据当前各项因素计算如果没有干扰，依据预设航线前进所需要的正常/恢复正常参数
        # 直接修改 head, speed, rot等
        # 本处采用简单决策，如果需要更精确的计算，可在子类中继承重写即可

        # 首先检查是否到达拐点范围，如果是，以下一拐点为目标，调整head，这里考虑匀速运动
        # 如果没有下一拐点（到达终点），则本船阶段仿真结束，船舶消失
        nextPoint = self.track[self.next]
        (targetLon, targetLat) = nextPoint
        if self.isReachPoint(targetLon, targetLat):
            # 到达拐点，获取下一拐点
            self.next = self.next + 1
            (targetLon, targetLat) = nextPoint

            # 检查是否到达终点，如果是，则本船阶段仿真结束
            if self.next == len(self.track):
                # 终点抵达，设置仿真结束标志
                self.stop = True

        # 以拐点为目标，调整head，这里仅仅考虑匀速运动，故而 speed 和 rot 不变
        # 由于大地圆球影响，不能继续沿用之前的head参数
        self.head = calcTrueHeading(self.lon, self.lat, targetLon, targetLat)
        '''
        # TODO
        super().NavigateModel()
        pass

    def ShipMotionModel(self):
        '''
        # 此方法可能不需要修改
        # 根据水动力参数，前进一步，修改新的lon,lat位置等参数
        # 默认：线性模型，如果需要更精确详细的计算，可参考水动力学模型后，在子类中继承重写即可
        # lon, lat: 起始坐标
        # speed: 航速，待统一转换，初步单位为 m/s
        # heding: 航向角，以真北为基准顺时针度量到航向线的角度
        # distance：本周期内，船舶行走的距离长度，初步单位为米
        # 返回值：新的坐标点
        '''
        '''
        #更新船舶航向和角速度
        rot_temp = self.rot_old + (self.K * self.delta - self.rot_old) / self.T
        self.rot_old = self.rot
        self.rot = rot_temp
        self.head += self.rot
        '''
        super().ShipMotionModel()

    def calcDCPA(self, targetShip):
        #计算本船和目标船的DCPA
        #本船从目标船船首穿过为负数，从船尾穿过为正数
        #单位为m
        # TODO
        DCPA = 0
        self.DCPA = DCPA
        pass

    def calcTCPA(self, targetShip):
        #计算本船和目标船的TCPA，
        #单位为s
        # TODO
        TCPA = 0
        self.TCPA = TCPA
        pass

    def CollisionRisk(self): #蔡明佑负责开发
        #计算本船与周围船舶的碰撞风险
        #确定以下参数：
        # TODO
        self.risk_ship = None      #碰撞风险最大船舶的MMSI
        self.collision_risk = None #碰撞风险值
        self.DCPA = None           #DCPA值
        self.TCPA = None           #TCPA值
        pass

    #计算两条船舶的交叉角度，单位为°
    def crossingAngle(self, self_course, target_course):
        #本船速度向量
        x_1 = math.sin(self_course * math.pi /180)
        y_1 = math.cos(self_course * math.pi /180)
        
        #目标船速度向量
        x_2 = math.sin(target_course * math.pi /180)
        y_2 = math.cos(target_course * math.pi /180)
        
        theta = math.arccos(x_1 * x_2 + y_1 * y_2) * 180 / math.pi
        return theta


def main():
    Ships = []
    aData = {'mmsi': 2130001, 'length':10.0, 'width': 5.0, 'lon': 118.67364, 'lat': 32.00683, 'speed': 12, #航速20节
             'track':[
                 [118.765052, 32.137083], [118.757578, 32.130112], [118.748092, 32.120204],
                      [118.739253, 32.109989], [118.730342, 32.098917], [118.724449, 32.092249],
                      [118.717047, 32.084723], [118.71101, 32.078665], [118.706698, 32.075178],
                      [118.701811, 32.06967], [118.698074, 32.063673], [118.694194, 32.054859],
                      [118.690313, 32.043107], [118.687151, 32.034537], [118.682049, 32.024496],
                      [118.67364, 32.00683], [118.669185, 31.997185], [118.663579, 31.988089],
                      [118.65783, 31.980157], [118.649135, 31.971795], [118.639864, 31.96426],
                      [118.632893, 31.958133], [118.623695, 31.951147], [118.616221, 31.946552],
                      [118.606303, 31.941894], [118.596674, 31.937482], [118.580935, 31.929208],
                      [118.572671, 31.923017], [118.566059, 31.915171], [118.556142, 31.904198],
                      [118.551399, 31.898741], [118.545291, 31.893468], [118.534223, 31.883167],
                      [118.529624, 31.877464], [118.525169, 31.870718], [118.521216, 31.861702],
                      [118.518557, 31.851397], [118.51712, 31.844035], [118.515251, 31.836366],
                      [118.513526, 31.832414], [118.511155, 31.828451], [118.505693, 31.821149]]}
    sShip1 = TSimShipFerry(aData)
    # sShip1 = TSimShipFerry()
    # sShip1.initData(aData)

    # 渡船
    bData = {'mmsi': 2130002, 'length':10.0, 'width': 5.0, 'lon': 118.67364, 'lat': 32.00683, 'speed': 12, #航速20节
             'track':[
                    [118.6161334635414, 31.949146662009515], [118.61616020822231, 31.94914974418367], 
                    [118.61624043863844, 31.949152328692392], [118.61633404057868, 31.94915812844723], 
                    [118.61648112604735, 31.949166781716954], [118.61662821202789, 31.9491854526172], 
                    [118.61686889872524, 31.949236649411088], [118.61706946677438, 31.94928824032799], 
                    [118.61733687328848, 31.94934264167453], [118.61764436398813, 31.949380119618446], 
                    [118.61805877178053, 31.94941019235194], [118.61840631023283, 31.94942424719987], 
                    [118.61887410651686, 31.94942424372517], [118.61926166798753, 31.94940163686082], 
                    [118.61972937900511, 31.949382011342745], [118.62074483020028, 31.94933630495799], 
                    [118.62118568021988, 31.94930413849431], [118.62322910100428, 31.949159917198404], 
                    [118.62378987765746, 31.949098571534822], [118.62424378723708, 31.94904104749371], 
                    [118.62475103698716, 31.94895700388447], [118.6252582283101, 31.94886655402719], 
                    [118.62575200888746, 31.94875636882634], [118.62621903630205, 31.948623091265766], 
                    [118.626726030053, 31.948456738060262], [118.62711289719509, 31.94830715528557], 
                    [118.62752639987866, 31.948131104116943], [118.6279265095161, 31.947925201979075], 
                    [118.62825990554843, 31.94775262924892], [118.62857993311223, 31.94757013341205], 
                    [118.62887326808064, 31.94739765883144], [118.62913991930583, 31.947241847673812], 
                    [118.62937989003338, 31.947099345461893], [118.62961985093894, 31.94696356678606], 
                    [118.629819813929, 31.94686101625199], [118.6302729672264, 31.946516283066213], 
                    [118.63039291599081, 31.94643349474365], [118.63048617208129, 31.946307267990136]]}

    sShip2 = TSimShipFerry(bData)
    # sShip2 = TSimShipFerry()
    # sShip2.initData(bData)

    Ships.append(sShip1)
    Ships.append(sShip2)
    # 可以在这里再添加其他的船，再将其模仿上述过程append到Ships中

    # main_fun(Ships)将自动运行，以下内容请勿修改
    def main_fun(Ships):
        flag = True # 标识是否有船没有运行完毕
        stack  = [] # 当前的船舶运行状态列表
        while flag:
            cache = {
                "Time": ''
            }
            for ship in Ships:
                stack.append(ship.stop)
                if not ship.stop:
                    # 每个船舶对象运行的状态
                    status = ship.Run() 
                    # cache.append(status)
                    cache['Time'] = status.get('Time')
                    cache[str(status.get('mmsi'))] = status
            pool.put(cache) # 将当前时刻的所有船舶状态以列表形式加入数据池
            status_stack = map(lambda x: 1 if x else 0, stack)
            count = reduce(lambda x, y: x + y, status_stack)
            if count  == len(stack):
                flag = False
            stack = []
            pass
    main_fun(Ships)
    pass


if __name__ == '__main__':
    main()
